#ifndef Quaternion_h
#define Quaternion_h

#include "Arduino.h"
#include <FP_math.h>

class Quaternion {
  
  /*  Implements a quaternion object
      Quaternion object is based on Fixed-Point Math
      Makes extensive use of the FP_math Fixed-point library
  */

  public:

    Quaternion();
    Quaternion(int q[]);
    Quaternion(int heading, int pitch, int roll);
    Quaternion(int w, int x, int y, int z);
  
    void setQuat(int heading, int pitch, int roll);  // Sets quaternion attitude
    void setQuat(int qw, int qx, int qy, int qz); 
    void setQuat(int q[]);

    int getW(void);
    int getX(void);
    int getY(void);
    int getZ(void);
    void getQ(int q[]);
  
    void multiply(int qw, int qx, int qy, int qz);  // Performs a quaternion multiply by q
    void multiply(int q[]);                         // i.e. q0 = q * q0
    void multiply(Quaternion q);                    // i.e. Rotates Attitude by quaternion q
  
    void postMultiply(int qw, int qx, int qy, int qz);  // Post-multiplies quaternion q
    void postMultiply(int q[]);            // i.e. Attitude = Attitude * q
    void postMultiply(Quaternion q);       // i.e. Rotates body by quaternion q

    void negMult(Quaternion a);  // Performs a quaternion multiply by q-1
    void negMult(int q[]);       // i.e. q0 = q^-1 * q0
    void negMult(int theW, int theX, int theY, int theZ);

    void rotationMatrix(int RMx[][3]);
    void setRotationMatrix(void);
  
    void ned2body(int r[], int outVector[]);            // Rotates NED vector r to body coordinates
    void body2ned(int r[], int outVector[]);            // Rotates Body vector r to NED coordinates
    void ned2bodyXY(int r[], int outVector[]);            // Rotates NED vector r to body coordinates
    void body2nedXY(int r[], int outVector[]);            // Rotates Body vector r to NED coordinates
    
    int scaleZ(void);
  
    void normalize();
    
    int R[3][3];

  protected:
    int w, x, y, z;
    
    boolean rSet;

};

class DifQuat : public Quaternion {
  /*
    DifQuat extends Quaternion to provide body angle difference between two quaternions
  */

  public:
    int dx, dy, dz; // These are difference angles in radians, Fix_16_14
  
    DifQuat();  
    DifQuat(int q[]);  
    DifQuat(int h, int p, int r);
    DifQuat(int q1, int q2, int q3, int q4);  

    void computeDiff(Quaternion a);  
    void computeDiff(int q[]);  
    void computeDiff(int theW, int theX, int theY, int theZ);

  protected:
    void zeroVals(void);
};

class IMU : public Quaternion {
  /*
    Implements a Quaternion Attitude IMU

    Includes method to propagate the Attitude quaternion based on 
    body-mounted gyro (angular rate) data, gyro scaling & a 32-bit
    integrator.
  */

  public:

    union split_long {
      long l;
      struct {
        int lo;
        int hi;
      } s;
    } w_int, x_int, y_int, z_int;  // The 32-bit integrators

    IMU();  
    IMU(int q[]);  
    IMU(int h, int p, int r);
    IMU(int q1, int q2, int q3, int q4); 
    
    void setQuat(int heading, int pitch, int roll);  // Sets quaternion attitude
    void setQuat(int qw, int qx, int qy, int qz); 
    void setQuat(int q[]);


    void setGyroScale(int scale);
      // Sets Gyro Scale factors for propagate() integrator
      // Rate (Radians/sec) = Gyro_Raw * scale * (2^-19)

    int heading(void); // Returns heading, deg10
    int pitch(void);   // Returns pitch, deg10
    int roll(void);    // Returns roll, deg10
  
    void propagate(int xDot, int yDot, int zDot, int dT);  
    void propagate(int vector[], int dT);
    void propagate(Vector3 vector, int dT);
      // Integrates body angular rates for dT
      // nDot values are raw sensor - scaled by setGyroScale(scale, exp2)
      // dT is in milliseconds

  protected:
    int gyroScale;
    void load_int(void);
    long normOne(int val, int norm);
};

#endif

